
#include "hardwareIO.h" // this is to have access to mirror limits among other things


//Scheduler

void schedulerInit(){
  schedulerMode = -1;
  schedulerTime =  millis()-SCHEDULERMODETIME; // this way, we are sure scheduler() will change the mode the first time
  scheduler();
}


//test time and call mode functions
void scheduler(){
  
  //test to know if it is time to change the interaction mode
  if (millis() - schedulerTime >=  SCHEDULERMODETIME) {
    //define the spots variables accordingly to the new interaction mode
    switch(schedulerMode){
      
      case -1: // this is a test mode of the borders:
       for(int i=0; i<NBSPOT; i++){
      spotArray[i].alive = false;
       }
      SCHEDULERMODETIME = 60000;
      schedulerMode++;
      schedulerTime = millis();
      break;
      
      
      case 0: // one contour following
      
      // transitionStep();
      simpleTransitionStep();
      contourFollowingOnly(1);
      spotArray[0].speedContourFollowing = 0.7; // controls the speed of contour following (proportion of radius)
      spotArray[0].showLimits=false; // avoid having the green contour when touching the "walls" (because we will project from the side). 
      spotArray[0].showGreenFlash=false;
      digitalWrite(greenLaserPin, LOW); // activate green laser
      // digitalWrite(greenLaserPin, HIGH); // laser off
      
      // AFTER setting the spot, set the threshold value and mode:
      spotArray[0].updateThreshold(-1); // -1 means autothreshold
      //Use potentiometer threshold:
      //potentiometerThresholdRead();
      //spotArray[0].updateThreshold(potFixThreshold);
      // ... or fixed threshold:
      //spotArray[0].updateThreshold(FIXED_THRESHOLD);
      
      // Change midi mode:
      myMidi.midiProgramChange (4, 1);
      
      // set schedule time:
      SCHEDULERMODETIME = 120000;//2 min to avoid long activation of green laser
      schedulerMode++;
      schedulerTime = millis();
      break;  
      
       case 1:
      //transitionStep();
      simpleTransitionStep();
      contourFollowingOnly(2);
      spotArray[0].showLimits=false; spotArray[1].showLimits=false;
        spotArray[0].showGreenFlash=false;spotArray[1].showGreenFlash=false;
      // make one spot turn in the opposite direction:
      spotArray[1].speedContourFollowing = -0.6; // controls the speed of contour following (proportion of radius)
      digitalWrite(greenLaserPin, HIGH); // laser off
      
       spotArray[1].updatePosition(3000, 3000);
      
      // AFTER setting the spot, set the threshold value and mode:
      spotArray[1].updateThreshold(-1); // -1 means autothreshold
      spotArray[0].updateThreshold(-1); 
      //Use potentiometer threshold:
      //potentiometerThresholdRead();
      //spotArray[0].updateThreshold(potFixThreshold);
      // ... or fixed threshold:
      //spotArray[0].updateThreshold(FIXED_THRESHOLD);
      
      myMidi.midiProgramChange (4, 4);
      
      SCHEDULERMODETIME = 180000; //3min
      schedulerMode++;
      schedulerTime = millis();
      break;
      
      
      
      case 2: 
     //  transitionStep();
      simpleTransitionStep();
       simpleBouncingOnly(1);
       spotArray[0].showLimits=false;
       spotArray[0].setPhoenixSpeed(16,5);  // New for seikai ichi: start speed is 0 and working speed will be loaded with firstTimeSpeedNorm (the fist input parameter) when collision==true for the first time. This mode
       // is reset when speed norm becomes smaller than the second parameter (this is to take into account damping that will make the particle stop again)
       
       spotArray[0].vx=8;
       spotArray[0].vy=7;
       spotArray[0].bounce_collisionDamping=1.00; // 1 means no damping when collision (when in bouncing mode). Larger than one, this means it will accelerate. Note: max speed is the initial "phoenix" speed!
       spotArray[0].bounce_movingDamping=0.9995; // 1 means no damping as it moves (when in bouncing mode). 
       spotArray[0].showGreenFlash=false; // false: faster
       spotArray[0].updatePosition(2000, 1500);
       digitalWrite(greenLaserPin, LOW); // laser activated
       
       // AFTER setting the spot, set the threshold value and mode:
       spotArray[0].updateThreshold(-1);
       
       SCHEDULERMODETIME = 120000; //2min
       schedulerMode++;
       schedulerTime = millis();
       break;
       
       
      case 3: 
     //  transitionStep();
       simpleTransitionStep();
       simpleBouncingOnly(2);
       spotArray[0].showLimits=false;
       spotArray[0].setPhoenixSpeed(16,5);  // New for seikai ichi: start speed is 0 and working speed will be loaded with firstTimeSpeedNorm (the fist input parameter) when collision==true for the first time. This mode
       // is reset when speed norm becomes smaller than the second parameter (this is to take into account damping that will make the particle stop again)
       spotArray[0].vx=8; spotArray[0].vy=7;
       spotArray[0].bounce_collisionDamping=1.00; // 1 means no damping when collision (when in bouncing mode). Larger than one, this means it will accelerate. Note: max speed is the initial "phoenix" speed!
       spotArray[0].bounce_movingDamping=0.9995; // 1 means no damping as it moves (when in bouncing mode). 
       spotArray[0].showGreenFlash=false; // false: faster
       spotArray[0].updatePosition(2000, 1500);
    
       
       spotArray[1].showLimits=false;
       spotArray[1].setPhoenixSpeed(16,5);  // New for seikai ichi: start speed is 0 and working speed will be loaded with firstTimeSpeedNorm (the fist input parameter) when collision==true for the first time. This mode
       // is reset when speed norm becomes smaller than the second parameter (this is to take into account damping that will make the particle stop again)
       spotArray[1].vx=-8; spotArray[1].vy=-7;
       spotArray[1].bounce_collisionDamping=1.00; // 1 means no damping when collision (when in bouncing mode). Larger than one, this means it will accelerate. Note: max speed is the initial "phoenix" speed!
       spotArray[1].bounce_movingDamping=0.9995; // 1 means no damping as it moves (when in bouncing mode). 
       spotArray[1].showGreenFlash=false; // false: faster
       spotArray[1].updatePosition(3000, 3000);
       
      // AFTER setting the spot, set the threshold value and mode:
      spotArray[0].updateThreshold(-1); // -1 means autothreshold
      spotArray[1].updateThreshold(-1); 
      //Use potentiometer threshold:
      //potentiometerThresholdRead();
      //spotArray[0].updateThreshold(potFixThreshold);
      // ... or fixed threshold:
      //spotArray[0].updateThreshold(FIXED_THRESHOLD);
       
       digitalWrite(greenLaserPin, HIGH); // laser off
       
       SCHEDULERMODETIME = 180000; //2min
       schedulerMode++;
       schedulerTime = millis();
       break;
      
     case 4:
       simpleTransitionStep();
       snellLawOnly(1);
       
       // AFTER setting the spot, set the threshold value and mode:
      //spotArray[0].updateThreshold(-1); // -1 means autothreshold
      //Use potentiometer threshold:
      potentiometerThresholdRead();
      spotArray[0].updateThreshold(potFixThreshold);
      // ... or fixed threshold:
      //spotArray[0].updateThreshold(FIXED_THRESHOLD);
       
       schedulerMode++;
       SCHEDULERMODETIME = 180000; //2min
       schedulerTime = millis();
       break;

/*
 case 3:
      //transitionStep();
      simpleTransitionStep();
      contourFollowingOnly(2);
      spotArray[0].showLimits=false; spotArray[1].showLimits=false;
        spotArray[0].showGreenFlash=false;spotArray[1].showGreenFlash=false;
      // make one spot turn in the opposite direction:
      spotArray[1].speedContourFollowing = -0.6; // controls the speed of contour following (proportion of radius)
      digitalWrite(greenLaserPin, HIGH); // laser off
      
       spotArray[1].updatePosition(3000, 3000);
      
      midiProgramChange (4, 3);
      
      SCHEDULERMODETIME = 180000; //3min
      schedulerMode++;
      schedulerTime = millis();
      break;
*/
     
      /*
      case 3:
     // transitionStep();
      simpleTransitionStep();
      simpleBouncingOnly(2);
      spotArray[0].showLimits=false; spotArray[1].showLimits=false;
      spotArray[0].setPhoenixSpeed(40,10); spotArray[1].setPhoenixSpeed(40,5); 
      spotArray[0].bounce_collisionDamping=1.04; spotArray[1].bounce_collisionDamping=1.06;
      spotArray[0].bounce_movingDamping=0.997;spotArray[1].bounce_movingDamping=0.99;
      spotArray[0].showGreenFlash=false;  spotArray[1].showGreenFlash=false;
      schedulerMode++;
      schedulerTime = millis();
      break;
      
      
      
      
     case 3:
      //transitionStep();
      contourFollowingOnly(3);
      spotArray[0].showLimits=false; spotArray[1].showLimits=false;spotArray[2].showLimits=false;
        spotArray[0].showGreenFlash=false;spotArray[1].showGreenFlash=false;spotArray[2].showGreenFlash=false;
      // make one spot turn in the opposite direction:
      spotArray[0].speedContourFollowing = 0.8; // controls the speed of contour following (proportion of radius)
      spotArray[1].speedContourFollowing = -0.6;
       spotArray[2].speedContourFollowing = 0.6;
      schedulerMode++;
      schedulerTime = millis();
      break;
      
      case 4: 
     //  transitionStep();
       simpleBouncingOnly(1);
       spotArray[0].showLimits=false;
       spotArray[0].setPhoenixSpeed(18,5);  // New for seikai ichi: start speed is 0 and working speed will be loaded with firstTimeSpeedNorm (the fist input parameter) when collision==true for the first time. This mode
       // is reset when speed norm becomes smaller than the second parameter (this is to take into account damping that will make the particle stop again)
       spotArray[0].bounce_collisionDamping=1.03; // 1 means no damping when collision (when in bouncing mode). Larger than one, this means it will accelerate. Note: max speed is the initial "phoenix" speed!
       spotArray[0].bounce_movingDamping=0.9995; // 1 means no damping as it moves (when in bouncing mode). 
       spotArray[0].showGreenFlash=false; // false: faster
       schedulerMode++;
       schedulerTime = millis();
       break;
       
      
     
      
      case 5:
     // transitionStep();
      simpleBouncingOnly(2);
      spotArray[0].showLimits=false; spotArray[1].showLimits=false;
      spotArray[0].setPhoenixSpeed(40,10); spotArray[1].setPhoenixSpeed(40,5); 
      spotArray[0].bounce_collisionDamping=1.04; spotArray[1].bounce_collisionDamping=1.06;
      spotArray[0].bounce_movingDamping=0.997;spotArray[1].bounce_movingDamping=0.99;
      spotArray[0].showGreenFlash=false;  spotArray[1].showGreenFlash=false;
      schedulerMode++;
      schedulerTime = millis();
      break;
      */
      
      /*
       case 3:
       transitionStep();
       
       //simple bouncing
       spotArray[0].Init(-1, 2000, 2000, 30 , 20, 0); 
       spotArray[0].vx = 30; 
       spotArray[0].vy = 30;
       spotArray[0].behaviourMode = SIMPLE_BOUNCING;
       spotArray[0].alive = true;
       
       //contour following
       spotArray[1].Init(-1, 2000, 2000, 30 , 20, 1); 
       spotArray[1].vx = 30; 
       spotArray[1].vy = 30;
       spotArray[1].behaviourMode = COUNTOUR_FOLLOWING;
       spotArray[1].alive = true;
       
       schedulerMode++;
       schedulerTime = millis();
       break;
       */
       
       
      /* 
       case 3:
       transitionStep();
       schedulerMode++;
       schedulerTime = millis();
       break;
       
       case 4:
       transitionStep();
       schedulerMode++;
       schedulerTime = millis();
       break;
       
       case 5:
       transitionStep();
       schedulerMode++;
       schedulerTime = millis();
       break;
       */
    default:
      schedulerMode = 0;
      //schedulerTime =  SCHEDULERMODETIME;
      break;
    } 
  }
}


// *** SIMPLE TRANSITION STEP: "Turn off sounds" *** //
void simpleTransitionStep(){
  //turn off all sounds
  myMidi.midiAllNoteOff();
}


// *** TRANSITION STEP: "Concentric Circles" *** //
void transitionStep(){
  
//turn off all sounds
myMidi.midiAllNoteOff();
  
  //draw max area and a saccade 
  Spimcp.writeOutR(0); // turn off the red laser
  
  /*
  // FreeBass CC PRM init:
  // CutOff Max:
  midiControlChange(1, 107, 127);
  // Rezo Null:
  midiControlChange(1, 106, 0);
  // Envelop Modulation null:
  midiControlChange(1, 104, 0);
  // Decay max:
  midiControlChange(1, 102, 127);
  // Disto nulle:
  midiControlChange(1, 105 , 0);
  */
      
  spotArray[0].setNumSamples(40);
  spotArray[0].updatePosition(2000, 2000);
  for(int l=0; l<2; l++){
    for(int i = 1500; i>=0; i-=100){
      spotArray[0].drawMaxArea(1);
      digitalWrite(greenLaserPin, LOW);
      spotArray[0].drawMaxArea(1);
      digitalWrite(greenLaserPin, HIGH);
      spotArray[0].updatePosition(2000, 2000);
      spotArray[0].computeNormalVector(2000, 2000, i);
      digitalWrite(greenLaserPin, LOW);
      spotArray[0].computeNormalVector(2000, 2000, i);
      digitalWrite(greenLaserPin, HIGH);
   
     /*
     // Modulation CutOff & Rezo:
      byte cutoff = (byte)(127- (127.0*i/1500.0));
      midiControlChange(1, 107, cutoff);
      byte rezo = (byte)(127- 127.0*i/1500.0);
      midiControlChange(1, 106, rezo);
      
      byte note = (byte)(48 - (24* i/1600));
      
      midiNoteOff(1, 30);
      midiNoteOn(1, 30, 100);
      
      */
      myMidi.midiNoteOff(10, 35);
      myMidi.midiNoteOn(10, 35, 127);
    
      }
    
    for(int i = 0; i<1500; i+=100){
      spotArray[0].drawMaxArea(1);
      digitalWrite(greenLaserPin, LOW);
      spotArray[0].drawMaxArea(1);
      digitalWrite(greenLaserPin, HIGH);
      spotArray[0].updatePosition(2000, 2000);
      spotArray[0].computeNormalVector(2000, 2000, i);
      digitalWrite(greenLaserPin, LOW);
      spotArray[0].computeNormalVector(2000, 2000, i);
      digitalWrite(greenLaserPin, HIGH);
      
      /*
      byte cutoff = (byte)(127- (127.0*i/1500.0));
      midiControlChange(1, 107, cutoff);
      byte rezo = (byte)(127 - 127.0*i/1500.0);
      midiControlChange(1, 106, rezo);
      
      byte note = (byte)(48- (24* i/1600));
     
      midiNoteOff(1, 30);
      midiNoteOn(1, 30, 100);
     */
      myMidi.midiNoteOff(10, 35);
      myMidi.midiNoteOn(10, 35, 127);
  
    }
    
    
  // Circle bounce on wall >> 1 note on FB33
  //midiNoteOff(0, 30);
  //midiNoteOn(0, 30, 127);
  }
  
  spotArray[0].setNumSamples(18);  
  Spimcp.writeOutR(4000);
}

//////////// MODE FUNCTIONS ////////////

void newtonianBouncingOnly(int nbSpots){
  for(int i=0; i<nbSpots; i++){
      spotArray[i].behaviourMode = NEWTON_DYNAMICS;
    
    //Init parameters: (int threshold, int posX, int posY, int saccadeRadius, int numSample, int id)
    spotArray[i].Init(-1, 2100+i*500, 1500+i*500, 30+8*i , 20, i); 
    
    // UpdateSpeed must be used when setting speed in NETWON_DYNAMICS mode, right after updatePosition (or Init of course).  
    spotArray[i].updatePosition(random(1000)+1000, random(1000)+1000); 
    spotArray[i].updateSpeed(-15,10); 
    
     spotArray[i].borderMode=NORMAL_REFLEXION;
    
    spotArray[i].bouncingForceMode=false; // default true
    spotArray[i].bouncingSpecularMode=false;
    spotArray[i].centralForceMode=false; // default mode is CENTRAL force active
    spotArray[i].parallelForceMode=true; // default mode is parallel force inactive
    spotArray[i].particleInteractionMode=true;// default mode 
    spotArray[i].visForceMode=true; // default mode false

    spotArray[i].alive = true;
    spotArray[i].mass=0.3;//+i*0.2;

  }  
  
  // Massive spot: 
   spotArray[0].updatePosition(2000, 2000); 
    spotArray[0].updateSpeed(-20, 10); 
   spotArray[0].mass=0.5;
    spotArray[0].charge=1; // it will attract harder if charge>1 (the others have charge=1)
  
  // kill all the other spots if they were alive: 
  for(int i=nbSpots; i<NBSPOT; i++){
    spotArray[i].alive = false;
  }
}

void simpleBouncingOnly(int nbSpots){
  for(int i=0; i<nbSpots; i++){
    //Init parameters: (int threshold, int posX, int posY, int saccadeRadius, int numSample, int id)
    spotArray[i].Init(-1, 2000,2000, 18, 20, i); 
    spotArray[i].behaviourMode = SIMPLE_BOUNCING;
    spotArray[i].vx = 18; 
    spotArray[i].vy = 18;
    spotArray[i].alive = true;
  }  
  for(int i=nbSpots; i<NBSPOT; i++){
    spotArray[i].alive = false;
  }
}

void fastBouncingOnly(int nbSpots){
  for(int i=0; i<nbSpots; i++){
    //Init parameters: (int threshold, int posX, int posY, int saccadeRadius, int numSample, int id)
    spotArray[i].Init(-1, 1000, 1000, 30+8*i , 20, i); 
    spotArray[i].behaviourMode = FAST_BOUNCING;
    spotArray[i].vx = 10; 
    spotArray[i].vy = 3;
    spotArray[i].alive = true;
  }  
  for(int i=nbSpots; i<NBSPOT; i++){
    spotArray[i].alive = false;
  }
}

void snellLawOnly(int nbSpots){
  for(int i=0; i<nbSpots; i++){
    //Init parameters: (int threshold, int posX, int posY, int saccadeRadius, int numSample, int id)
    spotArray[i].Init(-1, 1000, 1000, 30, 25, i);
    spotArray[i].error_angle = -0.25;
    spotArray[i].alive = true;
    spotArray[i].behaviourMode = SNELL_LAW;
    
    // behaviour at the borders:
    spotArray[i].borderMode =  RESET_POSITION;// RETRO_REFLEXION // NORMAL_REFLEXION;
    spotArray[i].placeX=550;  spotArray[i].placeY=CENTER_AD_MIRROR_Y; // special position where the spot appear after it touches the wall (when in RETRO_REFLEXION mode)
    spotArray[i].borderDamping=false;
    spotArray[i].showLimits=false;
     spotArray[i].showGreenFlash=false;
     
    spotArray[i].firstTimeStartRefraction=true; 
    spotArray[i].vx = 9;//10+8*i; 
    spotArray[i].vy = 0;//-8*i;
    spotArray[i].initVx = 9;//10+8*i; 
    spotArray[i].initVy = 0;//-8*i;

    //int xMax, yMax, xMin, yMin; //borde 
    
  }  
  for(int i=nbSpots; i<NBSPOT; i++){
    spotArray[i].alive = false;
  }
}


void contourFollowingOnly(int nbSpots){
  for(int i=0; i<nbSpots; i++){
    //Init parameters: (int threshold, int posX, int posY, int saccadeRadius, int numSample, int id)
    spotArray[i].Init(-1, 2000, 2000, 18 , 18, i); // 23 points per saccade is good
    spotArray[i].vx = 15; 
    spotArray[i].vy = 15;
    spotArray[i].behaviourMode = COUNTOUR_FOLLOWING;
    spotArray[i].speedContourFollowing = 0.7; // controls the speed of contour following (proportion of radius)
    spotArray[i].alive = true;
  }  
  for(int i=nbSpots; i<NBSPOT; i++){
    spotArray[i].alive = false;
  }
}

void allSpotsInit(){
  for(int i=0; i<NBSPOT; i++){
    //Init parameters: (int threshold, int posX, int posY, int saccadeRadius, int numSample, int id)
    spotArray[i].Init(-1, random(MAX_AD_MIRRORS), random(MAX_AD_MIRRORS), 18 , 18, i); // 23 points per saccade is good
    spotArray[i].vx = random(10,20); 
    spotArray[i].vy = random(10,20);
    spotArray[i].behaviourMode = SIMPLE_BOUNCING;//COUNTOUR_FOLLOWING;
    spotArray[i].bounce_movingDamping=1.0;
    spotArray[i].speedContourFollowing = 0.7; // controls the speed of contour following (proportion of radius)
    spotArray[i].alive = false;
    spotArray[i].showGreenFlash=false;
    spotArray[i].updateThreshold(-1); // -1 means autothreshold
    
    // The following is normally set in the constructor for soundSpot, but here we can tweak the default values:
     spotArray[i].sendSerialMode=false;
     spotArray[i].sendOSCMode=false;
     spotArray[i].sendMIDIMode=false;
     
  }  
    digitalWrite(greenLaserPin, LOW); // activate green laser
      // digitalWrite(greenLaserPin, HIGH); // laser OFF 
      
}


